#pragma once

#include <iostream>
#include <vector>
#include <string>
#include <cmath>
#include <memory>

#include <Eigen/Dense>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include "nanoflann.hpp"
#include "loop_closure/tictoc.h"

using SCPointType = pcl::PointXYZI; 

using namespace std;
using namespace Eigen;

// ======================== 核心修正-1: Nanoflann Adaptor ========================
// 为 std::vector<std::vector<float>> 数据类型创建一个适配器
// nanoflann需要这个适配器来理解我们的数据结构
using KeyMat = std::vector<std::vector<float>>;

struct KeyMat_Adaptor
{
    const KeyMat &obj;

    KeyMat_Adaptor(const KeyMat &obj_) : obj(obj_) {}

    inline size_t kdtree_get_point_count() const { return obj.size(); }

    inline float kdtree_get_pt(const size_t idx, const size_t dim) const {
        return obj[idx][dim];
    }

    template <class BBOX>
    bool kdtree_get_bbox(BBOX &) const { return false; }
};
// ==============================================================================

class SCManager
{
public: 
    static constexpr int    PC_NUM_RING = 20;
    static constexpr int    PC_NUM_SECTOR = 60;
    static constexpr double PC_MAX_RADIUS = 80.0;
    static constexpr double PC_UNIT_SECTORANGLE = 360.0 / double(PC_NUM_SECTOR);
    static constexpr int    NUM_EXCLUDE_RECENT = 30;
    static constexpr int    NUM_CANDIDATES_FROM_TREE = 10;
    static constexpr double SC_DIST_THRES = 0.5;
    static constexpr int    TREE_MAKING_PERIOD_ = 10;
    
    const double LIDAR_HEIGHT = 1.5; 

    // ======================== 核心修正-2: 修正KD-Tree类型定义 ========================
    // 使用我们新创建的适配器
    using InvKeyTree = nanoflann::KDTreeSingleIndexAdaptor<
        nanoflann::L2_Simple_Adaptor<float, KeyMat_Adaptor>,
        KeyMat_Adaptor,
        PC_NUM_RING
    >;
    // ==============================================================================
    
    SCManager();

    void makeAndSaveScancontextAndKeys(pcl::PointCloud<SCPointType> & _scan_down);
    std::pair<int, float> detectLoopClosureID(void);

private: 
    int tree_making_period_conter = 0;

    std::vector<Eigen::MatrixXd> polarcontexts_;
    std::vector<Eigen::MatrixXd> polarcontext_vkeys_;
    KeyMat polarcontext_invkeys_mat_;
    
    std::unique_ptr<InvKeyTree> polarcontext_tree_;

    MatrixXd makeScancontext(pcl::PointCloud<SCPointType> & _scan_down);
    MatrixXd makeRingkeyFromScancontext(Eigen::MatrixXd &_desc);
    MatrixXd makeSectorkeyFromScancontext(Eigen::MatrixXd &_desc);
    
    double distDirectSC (MatrixXd &_sc1, MatrixXd &_sc2);
    int fastAlignUsingVkey(MatrixXd & _vkey1, MatrixXd & _vkey2);
    std::pair<double, int> distanceBtnScanContext(MatrixXd &_sc1, MatrixXd &_sc2);

};